﻿#include "../control.h"
#include "blendmath.h"
//#include "quintic.h"
#include "speed_ctrl.h"
#include "tp_type.h"

extern STRUCT_CONTROL Ct;

//===============================
void spClear(STRUCT_SPEED_CTRL *vl)
{
	memset(vl, 0, sizeof(STRUCT_SPEED_CTRL));
}

//===============================
void spActive(STRUCT_SPEED_CTRL *vl,char is_active)
{
	vl->active = is_active;
	vl->current_vel = vl->req_start_vel;
	vl->finalvel = vl->req_stop_vel;
	vl->current_acc = 0;
	return;
}

//==================================
void spCalculateTrapezoidalAccel(STRUCT_SPEED_CTRL *vl, float *vel_desired)
{
	// Calculations for desired velocity based on trapezoidal profile
	float dx = (float)vl->target - (float)vl->progress;
	float maxaccel = vl->real_accel;
	float discr_term1 = Bl_Square(vl->real_final_vel);		//get real target vel
	float  discr_term2 = maxaccel * ((float)2.0 * dx - vl->current_vel * Ct.conf.inter_period);
	float tmp_adt = maxaccel * Ct.conf.inter_period * (float)0.5;
	float discr_term3 = Bl_Square(tmp_adt);
	float discr = discr_term1 + discr_term2 + discr_term3;
	float maxnewvel = -tmp_adt;
	if (discr > discr_term3){
		maxnewvel += blSqrt(discr);
	}
	float newvel = blSaturate((float)maxnewvel, vl->real_target_vel);
	// Calculate acceleration needed to reach newvel, bounded by machine maximum
	float dt = Ct.conf.inter_period;
	float maxnewaccel = (newvel - vl->current_vel) / dt;
	vl->current_acc = blSaturate(maxnewaccel, maxaccel);
	*vel_desired = maxnewvel;
	return;
}

//=============================
int spUpdateDistFromAccel(STRUCT_SPEED_CTRL *vl, float vel_desired)
{
	float v_next = (float)vl->current_vel + vl->current_acc * (float)Ct.conf.inter_period;
	if (v_next < 0) {
		v_next = 0.0;
		if ((vl->target - vl->progress) < (vl->current_vel *  Ct.conf.inter_period)) {
			vl->progress = vl->target;
		}
	}
	else {
		double displacement = (v_next + vl->current_vel) * 0.5 * Ct.conf.inter_period;
		vl->progress += displacement;
		blClipMax(&vl->progress, vl->target);
	}
	vl->current_vel = v_next;
	vl->on_final_decel = (fabs(vel_desired - vl->current_vel) < TP_SP_ONE_FINAL_DECEL_EPSILON) && (vl->current_acc <= 0.0);
	return 0;
}

